﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using MathNet.Numerics;
using System.Numerics;

namespace Microsoft.Samples.Kinect.BodyBasics.Calculator
{
    /// <summary>
    /// 使用复数计算
    /// </summary>
    public class RoboArmInverse
    {
        #region D-H
        //D-H参数
        private const double a1 = 0.01;
        private const double a2 = 0.076;
        private const double d4 = 0.107;
        private const double d6 = 0.091;
        #endregion

        #region 矩阵定义
        //矩阵
        private double mx = 1;
        private double nx = 0;
        private double px = 0;
        private double qx = 0;

        private double my = 0;
        private double ny = 1;
        private double py = 0;
        private double qy = 0;

        private double mz = 0;
        private double nz = 0;
        private double pz = 1;
        private double qz = 0;
        #endregion

        #region theta
        //角度定义复数
        private Complex theta1;

        private Complex theta21;
        private Complex theta22;

        private Complex theta31;
        private Complex theta32;

        private Complex theta41;
        private Complex theta42;

        private Complex theta51;
        private Complex theta52;

        private Complex theta61;
        private Complex theta62;
        #endregion

        #region 临时变量

        private Complex k1;
        private Complex k2;
        private Complex x1;
        private Complex x2;
        private Complex x3;
        #endregion

        #region 函数包装方法
        /// <summary>
        /// System.Numeric.Complex
        /// </summary>
        private Complex atan(System.Numerics.Complex complex)
        {
            return Trig.Atan(complex);
        }
        private Complex tan(Complex complex)
        {
            return Trig.Tan(complex);
        }
        private Complex acos(Complex complex)
        {
            return Trig.Acos(complex);
        }
        private Complex cos(Complex complex)
        {
            return Trig.Cos(complex);
        }
        private Complex asin(Complex complex)
        {
            return Trig.Asin(complex);
        }
        private Complex sin(Complex complex)
        {
            return Trig.Sin(complex);
        }
        private Complex sqrt(Complex complex)
        {
            return ComplexExtensions.SquareRoot(complex);
        }
        #endregion

        #region Sub_Calculate
        private void cal_theta1()
        {
            theta1 = atan((d6 * py - qy) / (d6 * px - qx));
            //Console.WriteLine("theta1: " + theta1);
        }
        private void cal_theta2()
        {
            k1 = d6 * (px * cos(theta1) + py * sin(theta1)) - (qx * cos(theta1) + qy * sin(theta1)) + a1;
            k2 = d6 * pz - qz;
            x1 = k1;
            x2 = k2;
            x3 = (d4 * d4 - k1 * k1 - k2 * k2 - a2 * a2) / (2 * a2);
            Complex temp = (x1 * x1 * x3 * x3) + (x2 * x2 * x3 * x3) - ComplexExtensions.Power(x3, 4);
            //Console.WriteLine("temp: " + temp);
            theta21 = atan((-x1 * x2 - sqrt(temp)) / (x2 * x2 - x3 * x3));
            theta22 = atan((-x1 * x2 + sqrt(temp)) / (x2 * x2 - x3 * x3));
            //Console.WriteLine("theta21: " + theta21 + " theta22: " + theta22);
        }
        private void cal_theta3()
        {
            theta31 = atan((k1 * cos(theta21) + k2 * sin(theta21) + a2) / (k1 * sin(theta21) - k2 * cos(theta21)));
            theta32 = atan((k1 * cos(theta22) + k2 * sin(theta22) + a2) / (k1 * sin(theta22) - k2 * cos(theta22)));
        }
        //先计算theta5
        private void cal_theta5()
        {
            theta51 = acos(-px * cos(theta1)
                * cos(theta21) * sin(theta31) - py * sin(theta1) * cos(theta21) * sin(theta31) - pz * sin(theta21)
                * sin(theta31) - px * cos(theta1) * sin(theta21) * cos(theta31) - py * sin(theta1) * sin(theta21) *
                cos(theta31) + pz * cos(theta21) * cos(theta31));
            theta52 = acos(-px * cos(theta1) *
                cos(theta22) * sin(theta32) - py * sin(theta1) * cos(theta22) * sin(theta32) -
                pz * sin(theta22) * sin(theta32) - px * cos(theta1) * sin(theta22) * cos(theta32) -
                py * sin(theta1) * sin(theta22) * cos(theta32) + pz * cos(theta22) * cos(theta32));
        }

        private void cal_theta4()
        {
            if (theta51.Real != 0)
            {
                theta41 = atan((-px * sin(theta1) + py * cos(theta1)) / (px * cos(theta1) * cos(theta21) * cos(theta31)
                    + py * sin(theta1) * cos(theta21) * cos(theta31) +
                    pz * sin(theta21) * cos(theta31) - px * cos(theta1) * sin(theta21) * sin(theta31) -
                    py * sin(theta1) * sin(theta21) * sin(theta31) + pz * cos(theta21) * sin(theta31)));

            }
            else
            {
                theta41 = new Complex(0, 0);
            }

            if (theta52.Real != 0)
            {
                theta42 = atan((-px * sin(theta1) + py * cos(theta1)) / (px * cos(theta1) * cos(theta22) * cos(theta32) +
                    py * sin(theta1) * cos(theta22) * cos(theta32) + pz * sin(theta22) * cos(theta32) -
                    px * cos(theta1) * sin(theta22) * sin(theta32) - py * sin(theta1) * sin(theta22) * sin(theta32)
                    + pz * cos(theta22) * sin(theta32)));

            }
            else
            {
                theta42 = new Complex(0, 0);
            }
        }
        private void cal_theta6()
        {
            if (theta51.Real != 0)
            {
                theta61 = atan((nx * cos(theta1) * cos(theta21) * cos(theta31) + ny * sin(theta1) * cos(theta21) * sin(theta31)
                    + nz * sin(theta21) * sin(theta31) + nx * cos(theta1) * sin(theta21) * cos(theta31) +
                    ny * sin(theta1) * sin(theta21) * cos(theta31)
                    - nz * cos(theta21) * cos(theta31)) / (-mx * cos(theta1) * cos(theta21) * sin(theta31) -
                    my * sin(theta1) * cos(theta21) * sin(theta31) - mz * sin(theta21) * sin(theta31) -
                    mx * cos(theta1) * sin(theta21) * cos(theta31) - my * sin(theta1) * sin(theta21) * cos(theta31) +
                    mz * cos(theta21) * cos(theta31)));

            }
            else
            {
                theta62 = new Complex(0, 0);
            }

            if (theta52.Real != 0)
            {
                theta62 = atan((nx * cos(theta1) * cos(theta22) * cos(theta32) +
                    ny * sin(theta1) * cos(theta22) * sin(theta32) + nz * sin(theta22) * sin(theta32) +
                    nx * cos(theta1) * sin(theta22) * cos(theta32) + ny * sin(theta1) * sin(theta22) * cos(theta32)
                    - nz * cos(theta22) * cos(theta32)) / (-mx * cos(theta1) * cos(theta22) * sin(theta32) -
                    my * sin(theta1) * cos(theta22) * sin(theta32) - mz * sin(theta22) * sin(theta32) -
                    mx * cos(theta1) * sin(theta22) * cos(theta32) - my * sin(theta1) * sin(theta22) * cos(theta32) +
                    mz * cos(theta22) * cos(theta32)));

            }
            else
            {
                theta62 = new Complex(0, 0);
            }
        }
        #endregion

        #region All_Calculate
        public Complex[] Calculate(float[] qArray)
        {
            double[] d_qAyyay = float2double_withTime(qArray, 1.5);
            setQArray(d_qAyyay);
            cal_theta1();
            cal_theta2();
            cal_theta3();
            cal_theta5();
            cal_theta4();
            cal_theta6();
            Complex[] thetas = new Complex[] { theta1, theta22, theta32, theta42, theta52, theta62 };
            Complex[] result = rad2deg(thetas);
            return result;
        }

        #endregion

        #region Transfer
        private double[] float2double_withTime(float[] floats, double times)
        {
            double[] result = new double[floats.Length];
            for (int i = 0; i < floats.Length; i++)
            {
                result[i] = (double)floats[i] * times;
            }
            return result;
        }
        private void setQArray(double[] qArray)
        {
            qx = qArray[0];
            qy = qArray[1];
            qz = qArray[2];
        }
        private Complex[] rad2deg(Complex[] complex)
        {
            Complex[] result = new Complex[complex.Length];
            for(int i = 0; i < complex.Length; i++)
            {
                result[i] = complex[i] * 57.3;
            }
            return result;
        }
        #endregion
    }
}
